close all; clear; clc;

%% base
syms px py pz rx ry rz real
syms delta_px delta_py delta_pz delta_rz delta_ry delta_rx real

Tb0 = Translx(px) * Transly(py) * Translz(pz) * Trotz(rz) * Troty(ry) * Trotx(rx);

pTb0_ppx = diff(Tb0, px);
pTb0_ppy = diff(Tb0, py);
pTb0_ppz = diff(Tb0, pz);
pTb0_prz = diff(Tb0, rz);
pTb0_pry = diff(Tb0, ry);
pTb0_prx = diff(Tb0, rx);

Q_px = simplify(inv(Tb0) * pTb0_ppx);
Q_py = simplify(inv(Tb0) * pTb0_ppy);
Q_pz = simplify(inv(Tb0) * pTb0_ppz);
Q_rz = simplify(inv(Tb0) * pTb0_prz);
Q_ry = simplify(inv(Tb0) * pTb0_pry);
Q_rx = simplify(inv(Tb0) * pTb0_prx);

delta_T = Q_px * delta_px + Q_py * delta_py + Q_pz * delta_pz ...
    + Q_rz * delta_rz + Q_ry * delta_ry + Q_rx * delta_rx;
delta_T = simplify(delta_T);

%% tool
syms tx ty tz real
syms delta_tx delta_ty delta_tz real

Ttool = Transl(tx, ty, tz);
pTtool_ptx = diff(Ttool, tx);
pTtool_pty = diff(Ttool, ty);
pTtool_ptz = diff(Ttool, tz);

Q_tx = simplify(inv(Ttool) * pTtool_ptx);
Q_ty = simplify(inv(Ttool) * pTtool_pty);
Q_tz = simplify(inv(Ttool) * pTtool_ptz);

delta_Ttool = Q_tx * delta_tx + Q_ty * delta_ty + Q_tz * delta_tz;

%%
Tb0 = Translx(px) * Transly(py) * Translz(pz) * Trotz(rz) * Troty(ry) * Trotx(rx);
Ttool = Transl(tx, ty, tz);

%% 
syms nx_r ny_r nz_r ox_r oy_r oz_r ax_r ay_r az_r px_r py_r pz_r real
T_robot = [nx_r, ox_r, ax_r, px_r;
    ny_r, oy_r, ay_r, py_r;
    nz_r, oz_r, az_r, pz_r;
    0, 0, 0, 1];

%%
delta_Tb0 = calculate_delta_Tb(px, py, pz, rz, ry, rx, delta_px, delta_py, delta_pz, delta_rz, delta_ry, delta_rx);
delta_Ttool = calculate_delta_Ttool(tx, ty, tz, delta_tx, delta_ty, delta_tz);

%%
dT = zeros(4, 4);
dT = simplify(dT + Tb0 * delta_Tb0 * T_robot * Ttool);
dT = simplify(dT + Tb0 * T_robot * Ttool * delta_Ttool);

%% 
px = dT(1, 4);
py = dT(2, 4);
pz = dT(3, 4);

%% 
dpx_dpx = simplify(diff(px, delta_px)); dpx_dpy = simplify(diff(px, delta_py)); dpx_dpz = simplify(diff(px, delta_pz));
dpx_drz = simplify(diff(px, delta_rz)); dpx_dry = simplify(diff(px, delta_ry)); dpx_drx = simplify(diff(px, delta_rx));
dpx_dtx = simplify(diff(px, delta_tx)); dpx_dty = simplify(diff(px, delta_ty)); dpx_dtz = simplify(diff(px, delta_tz));

dpy_dpx = simplify(diff(py, delta_px)); dpy_dpy = simplify(diff(py, delta_py)); dpy_dpz = simplify(diff(py, delta_pz));
dpy_drz = simplify(diff(py, delta_rz)); dpy_dry = simplify(diff(py, delta_ry)); dpy_drx = simplify(diff(py, delta_rx));
dpy_dtx = simplify(diff(py, delta_tx)); dpy_dty = simplify(diff(py, delta_ty)); dpy_dtz = simplify(diff(py, delta_tz));

dpz_dpx = simplify(diff(pz, delta_px)); dpz_dpy = simplify(diff(pz, delta_py)); dpz_dpz = simplify(diff(pz, delta_pz));
dpz_drz = simplify(diff(pz, delta_rz)); dpz_dry = simplify(diff(pz, delta_ry)); dpz_drx = simplify(diff(pz, delta_rx));
dpz_dtx = simplify(diff(pz, delta_tx)); dpz_dty = simplify(diff(pz, delta_ty)); dpz_dtz = simplify(diff(pz, delta_tz));

%%
error_px = px - (dpx_dpx * delta_px + dpx_dpy * delta_py + dpx_dpz * delta_pz ...
    + dpx_drz * delta_rz + dpx_dry * delta_ry + dpx_drx * delta_rx ...
    + dpx_dtx * delta_tx + dpx_dty * delta_ty + dpx_dtz * delta_tz);
error_py = py - (dpy_dpx * delta_px + dpy_dpy * delta_py + dpy_dpz * delta_pz ...
    + dpy_drz * delta_rz + dpy_dry * delta_ry + dpy_drx * delta_rx ...
    + dpy_dtx * delta_tx + dpy_dty * delta_ty + dpy_dtz * delta_tz);
error_pz = pz - (dpz_dpx * delta_px + dpz_dpy * delta_py + dpz_dpz * delta_pz ...
    + dpz_drz * delta_rz + dpz_dry * delta_ry + dpz_drx * delta_rx ...
    + dpz_dtx * delta_tx + dpz_dty * delta_ty + dpz_dtz * delta_tz);

%%
M = [dpx_dpx, dpx_dpy, dpx_dpz, dpx_drz, dpx_dry, dpx_drx, dpx_dtx, dpx_dty, dpx_dtz;
    dpy_dpx, dpy_dpy, dpy_dpz, dpy_drz, dpy_dry, dpy_drx, dpy_dtx, dpy_dty, dpy_dtz;
    dpz_dpx, dpz_dpy, dpz_dpz, dpz_drz, dpz_dry, dpz_drx, dpz_dtx, dpz_dty, dpz_dtz];
M = simplify(M);